/*
 * @Author: huangyupei huangyupei2021@ia.ac.cn
 * @Date: 2024-11-03 20:59:23
 * @LastEditors: huangyupei huangyupei2021@ia.ac.cn
 * @LastEditTime: 2024-11-20 10:51:49
 * @FilePath: /SelfCalib_OptiAcoustic/include/hand_eye_calibration.h
 * @Description: 这是默认设置,请设置`customMade`, 打开koroFileHeader查看配置 进行设置: https://github.com/OBKoro1/koro1FileHeader/wiki/%E9%85%8D%E7%BD%AE
 */
#pragma once

#include <iostream>
#include <vector>
#include <Eigen/Dense>
#include <opencv2/core.hpp>
#include <opencv2/imgproc.hpp>
#include <opencv2/highgui.hpp>
#include <opencv2/calib3d.hpp>
#include <math.h>
#include "sophus/se3.hpp"

using namespace std;
using namespace Eigen;

// 使用Eigen库求解手眼标定
// A_list: 机器人的位姿变化
// B_list: 相机的位姿变化
// X: 手眼标定结果，即相机坐标系到机器人坐标系的转换矩阵

void handEyeCalibration(const std::vector<Eigen::Matrix4d> &A_list,
                        const std::vector<Eigen::Matrix4d> &B_list,
                        Eigen::Matrix3d &R, Eigen::Vector3d &pm);

void handEyeCalibration(const std::vector<Eigen::Matrix4d> &A_list,
                        const std::vector<Eigen::Matrix4d> &B_list,
                        Eigen::Matrix3d &R, Eigen::Vector3d &pm, double &scale);



// 计算矩阵的伪逆
Eigen::MatrixXd pseudoinverse(const Eigen::MatrixXd& A, double tol = 1e-9);

std::pair<Eigen::Vector3d, double> getRotationAxis(const Eigen::Matrix3d& R) {
    // 计算旋转角度 theta
    double theta = acos((R.trace() - 1) / 2.0);
    
    // 特殊情况：theta = 0，没有旋转
    if (std::abs(theta) < 1e-6) {
        return {Eigen::Vector3d::Zero(), 0.0};
    }

    // 计算旋转轴
    Eigen::Vector3d axis;
    axis << R(2, 1) - R(1, 2),
            R(0, 2) - R(2, 0),
            R(1, 0) - R(0, 1);
    axis.normalize(); // 单位化旋转轴

    // 特殊情况：theta = pi，需要处理旋转轴
    if (std::abs(theta - M_PI) < 1e-6) {
        // 找到对应特征值为 1 的特征向量
        Eigen::EigenSolver<Eigen::Matrix3d> es(R);
        for (int i = 0; i < 3; ++i) {
            if (std::abs(es.eigenvalues()[i].real() - 1.0) < 1e-6) {
                axis = es.eigenvectors().col(i).real().normalized();
                break;
            }
        }
    } else {
        // 常规情况，通过反对称部分求解旋转轴
        axis /= (2 * sin(theta));
        axis.normalize(); // 单位化旋转轴
    }

    return {axis, theta};
}

void calcRelativeMotion(vector<Sophus::SE3d> vCamPoses, vector<Sophus::SE3d> vSonarPoses, vector<Eigen::Matrix4d> &cam_list, vector<Eigen::Matrix4d> &sonar_list);

double estimateScale(const Eigen::Vector3d& v1, const Eigen::Vector3d& v2);

double estimateScale(const std::vector<Eigen::Vector3d>& vA, const std::vector<Eigen::Vector3d>& vB);

void providePoseList(std::vector<Eigen::Matrix4d> &C_list, std::vector<Eigen::Matrix4d> &S_list) {
    Eigen::Matrix4d C12, C23, C34, C45, C56, C67;
    Eigen::Matrix4d S12, S23, S34, S45, S56, S67;
    
    C12 << 0.750000, 0.433013, -0.500000, 1.500000,
            -0.500000, 0.866025, 0.000000, 0.000000,
            0.433013, 0.250000, 0.866025, 0.500000,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C12);

    S12 << 0.750000, -0.500000, -0.433013, 1.375000,
            0.433013, 0.866025, -0.250000, 0.716506,
            0.500000, 0.000000, 0.866025, 0.250000,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S12);


    C23 << 0.399519, -0.808013, -0.433013, 1.183013,
            0.808013, 0.533494, -0.250000, 0.683013,
            0.433013, -0.250000, 0.866025, 0.366025,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C23);
    S23 << 0.399519, -0.433013, 0.808013, 0.882772,
            0.433013, 0.866025, 0.250000, 0.582532,
            -0.808013, 0.250000, 0.533494, -1.087019,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S23);


    C34 << 0.808013, -0.399519, -0.433013, 1.125000,
            0.533494, 0.808013, 0.250000, -0.649519,
            0.250000, -0.433013, 0.866025, 0.750000,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C34);
    S34 << 0.808013, -0.433013, 0.399519, 1.029006,
            0.250000, 0.866025, 0.433013, 0.875000,
            -0.533494, -0.250000, 0.808013, 0.382772,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S34);


    C45 << -0.649519, 0.625000, 0.433013, -1.250000,
            0.125000, 0.649519, -0.750000, 2.165064,
            -0.750000, -0.433013, -0.500000, 4.000000,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C45);
    S45 << -0.649519, 0.433013, -0.625000, -2.074760,
            -0.750000, -0.500000, 0.433013, 3.625000,
            -0.125000, 0.750000, 0.649519, -2.227564,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S45);


    C56 << 0.399519, -0.808013, 0.433013, -1.183013,
            0.808013, 0.533494, 0.250000, -0.683013,
            -0.433013, 0.250000, 0.866025, 0.366025,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C56);
    S56 << 0.399519, 0.433013, 0.808013, -1.483253,
            -0.433013, 0.866025, -0.250000, 0.149519,
            -0.808013, -0.250000, 0.533494, 0.279006,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S56);


    C67 << 0.808013, -0.399519, 0.433013, -1.125000,
            0.533494, 0.808013, -0.250000, 0.649519,
            -0.250000, 0.433013, 0.866025, 0.750000,
            0.000000, 0.000000, 0.000000, 1.000000;
    C_list.push_back(C67);
    S67 << 0.808013, 0.433013, 0.399519, -1.220994,
            -0.250000, 0.866025, -0.433013, 0.625000,
            -0.533494, 0.250000, 0.808013, -0.916266,
            0.000000, 0.000000, 0.000000, 1.000000;
    S_list.push_back(S67);
}
